

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.ExampleCommand;
import edu.wpi.first.wpilibj.templates.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;  
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;        
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.templates.commands.SetArcadeDrive2;
import edu.wpi.first.wpilibj.templates.commands.SetGameDrive;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.templates.commands.*;


public class RobotTemplate extends IterativeRobot {

   
   public static final Joystick drivestick = new Joystick(1); 
   public static final Joystick turretstick = new Joystick(2);
   public static JoystickButton drivebutton = new JoystickButton(drivestick, 7);
   public static JoystickButton armbuttonup = new JoystickButton(drivestick, 8);
 
  public static JoystickButton armbuttondown = new JoystickButton(drivestick, 5);
   public static Joystick armstick = new Joystick(3);
   public static JoystickButton arcadeDriveButton = new JoystickButton(drivestick, 9);
   public static JoystickButton gameDriveButton = new JoystickButton(drivestick, 10);
   public static JoystickButton turretbuttonleft = new JoystickButton(turretstick, 1); 
   public static JoystickButton turretbuttonright = new JoystickButton(turretstick, 2);
   public static JoystickButton firebutton = new JoystickButton(turretstick, 3);
   
 // public static Victor turretMotor = new Victor(6);
   
    
    
    
    
  
    

    public void robotInit() {
        
      

      
        CommandBase.init();
    }

    public void autonomousInit() {
        

    }

    
    public void autonomousPeriodic() {
  
    }

    public void teleopInit() {
	
         Scheduler.getInstance().run();
     arcadeDriveButton.whenPressed(new SetArcadeDrive2());
     gameDriveButton.whenPressed(new SetGameDrive());
     RobotTemplate.armbuttonup.whileHeld(new CutSpeed());
     RobotTemplate.armbuttonup.whenReleased(new NormalSpeed());
    }

   
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
}
